#define OPENCV
#include "udp.hpp"
#include <thread>
#include <opencv2/opencv.hpp>
#include <atomic>
#include <stdlib.h>
#include "mav_msg.h"
#include "Aitech.hpp"
#include <time.h>
#define printmsg(sg) std::cout << sg << std::endl
#if defined(_MSC_VER) && (_MSC_VER >= 1200)
#define dxnum 1.0
#include <Windows.h>
#else
#define dxnum 0.0001
#include <unistd.h>
#define Sleep(x) usleep(x * 1000)
#endif
/*
timechip
pic handle
net handle
detect handle

*/
//using namespace udpio;
//using namespace std;
#define robotmessage "Build Time:" + string(__TIME__) + " Date:" + __DATE__ + "\nCopyRight@2019-2020\nAUV(zzu) is connected.\nStart to send messages..."

int main(int argc, char *argv[])
{
	using namespace udpio;
	using namespace asio;
	using namespace std;
	using namespace cv;
	int options[5] = {0};
	for (size_t i = 1; i < argc; i++)
	{
		options[i - 1] = atoi(argv[i]);
	}

	FileStorage fs("data.xml", FileStorage::READ);
	Mat intrinsics, distortion, map1, map2;

	fs["camera_matrix"] >> intrinsics;
	fs["distortion_coefficients"] >> distortion;
	fs.release();

	initUndistortRectifyMap(
		intrinsics, distortion, Mat(),
		getOptimalNewCameraMatrix(intrinsics, distortion, Size(640, 480), 1, Size(1000, 750), 0),
		Size(1000, 750),
		CV_16SC2, map1, map2);
	Detector *detector = new Detector("auv.cfg", "auv.tech");

	try
	{
		/*

------------------------------------------------------------------------------------------------------
*/
		io_context io;

		udpnet controler(io, ip::udp::endpoint(ip::udp::v4(), 11524));
		atomic_bool is_connected(false);
		atomic_int cameranum(0);
		atomic_int videofps(30);

/*

------------------------------------------------------------------------------------------------------
*/
#define UDP_MODE

#ifdef UDPMODE
		asio::ip::udp::socket udp(io, asio::ip::udp::endpoint(asio::ip::address_v4(), 14560));
		udp.connect(asio::ip::udp::endpoint(asio::ip::address_v4::from_string("127.0.0.1"), 14570));
		mav::mavio mav(udp);
#else
		asio::serial_port serial(io, serial_port_name);
		if (!serial.is_open())
		{
			cout << "can not open serial files, please check it if this port is opened.\n Auv program is allready running but there is a error holds it back!" << endl;
		}
		mav::mavio mav(serial);
#endif
		mavlink_channel_t chan = MAVLINK_COMM_0;
		mav::bind_mavio_to(chan, mav);
		/*
		------------------------------------------------------------------------------------------------------
		*/
		string mav_errmsg;
		typedef struct
		{
			int x = 0;
			int y = 0;
			int z = 500;
			int r = 0;
		} vector_state;

		typedef struct
		{
			float pitch;
			float roll;
			float yaw;
			float deep;
		} zitai;
		zitai zitaimsg;

		//int lastcameranum=0;
		cv::VideoCapture cap(0);
		cv::VideoCapture cap2(2);
		cv::VideoCapture *camera = &cap;
		if (cap.isOpened())
		{
			printmsg("camera 1 is opened");
		}
		else
		{
			printmsg("camera 1 is not opened");
		}
		if (cap2.isOpened())
		{
			printmsg("camera 2 is opened");
		}
		else
		{
			printmsg("camera 2 is not opened");
		}
		auto changecamera = [&](int canum) {
			if (canum == 0)
			{
				camera = &cap;
			}
			else
			{
				camera = &cap2;
			}
		};

		std::atomic<float> pitchmsg(0);
		std::atomic<float> rollmsg(0);
		std::atomic<float> yawmsg(0);
		std::atomic<float> deepmsg(0);
		std::atomic<int> custom_mode(0);
		std::atomic<int> base_mode(0);
		std::atomic<int> battery_v(0);
		std::atomic<bool> canloaddetector(true);
		auto recfunc = [&](unsigned char buf) {
			//std::cout << (int)buf << std::endl;
			mavlink_message_t msg;
			mavlink_status_t status;
			if (mavlink_parse_char(0, buf, &msg, &status))
			{

				if (msg.msgid == 0)
				{
					mavlink_heartbeat_t ht;
					mavlink_msg_heartbeat_decode(&msg, &ht);
					custom_mode = ht.custom_mode;
					base_mode = ht.base_mode;
					//std::cout << "beat:custom_mode:" << ht.custom_mode << "base_mode:" << (int)v << std::endl;
				}
				if (msg.msgid == 253)
				{
					mavlink_statustext_t tp;
					mavlink_msg_statustext_decode(&msg, &tp);
					tp.text[49] = 0;
					mav_errmsg = string(tp.text);
					//std::cout << tp.text << std::endl;
				}
				else if (msg.msgid == 77)
				{
					mavlink_command_ack_t ta;
					mavlink_msg_command_ack_decode(&msg, &ta);
					//std::cout << ta.command << ":" << (int)ta.result << std::endl;
				}
				else if (msg.msgid == 65)
				{
					mavlink_rc_channels_t trc;
					mavlink_msg_rc_channels_decode(&msg, &trc);
					//std::cout << trc.chan1_raw << "  " << trc.chan2_raw << "  " << trc.chan3_raw << "  " << trc.chan4_raw << std::endl;
				}
				else if (msg.msgid == 30)
				{
					mavlink_attitude_t attitude;
					mavlink_msg_attitude_decode(&msg, &attitude);
					//	std::cout << "\b\b\b\b\b\b\b\b";
					//std::cout << attitude.pitch << ":" << attitude.roll << ":" << attitude.yaw;
					pitchmsg = attitude.pitch;
					rollmsg = attitude.roll;
					yawmsg = attitude.yaw;
				}
				else if (msg.msgid == 74)
				{
					mavlink_vfr_hud_t vfr_hud;
					mavlink_msg_vfr_hud_decode(&msg, &vfr_hud);
					deepmsg = vfr_hud.alt;
					//std::cout << "��ȣ�" << vfr_hud.alt << std::endl;
				}
				else if (msg.msgid == 1)
				{
					mavlink_sys_status_t sys_ss;
					mavlink_msg_sys_status_decode(&msg, &sys_ss);
					battery_v = sys_ss.voltage_battery;
				}
				else if (msg.msgid != 0 && msg.msgid != 251)
				{
					//std::cout << (int)msg.msgid << std::endl;
				}
			}
		};
		mav.setReceivedFunc(recfunc);
		/*

------------------------------------------------------------------------------------------------------
*/
		asio::steady_timer pixbeater(io);
		function<void(std::error_code)> pixbeatfunc = [&](std::error_code err) {
			mavlink_msg_heartbeat_send(chan, 1, MAV_AUTOPILOT_GENERIC, 0, 0, 0);
			pixbeater.expires_from_now(asio::chrono::milliseconds(700));
			pixbeater.async_wait(pixbeatfunc);
		};
		pixbeatfunc(std::error_code());

		/*
------------------------------------------------------------------------------------------------------
*/
		asio::steady_timer beattimer(io);
		function<void(std::error_code err, int time)> beatfunc = [&](std::error_code err, int time) {
			controler.send("beat", 0);
			//printmsg("beating");
			if (!is_connected)
				return;
			beattimer.expires_from_now(asio::chrono::milliseconds(time));
			beattimer.async_wait(bind(beatfunc, std::placeholders::_1, time));
		};

		auto start_beating_until_close = [&](int time) {
			beattimer.expires_from_now(asio::chrono::milliseconds(time));
			beattimer.async_wait(bind(beatfunc, std::placeholders::_1, time));
		};

		/*
------------------------------------------------------------------------------------------------------
*/
		asio::steady_timer killtimer(io);
		auto close_connect_while_no_beat_for = [&](int time) {
			killtimer.expires_from_now(asio::chrono::milliseconds(time));
			killtimer.async_wait([&](std::error_code err) {
				//printmsg(err);
				if (!err)
					if (is_connected) {
					is_connected = false; 
					printmsg("Connect lost");
				} });
		};
		/*

------------------------------------------------------------------------------------------------------
*/
		auto arm_f = [&](bool arm) {
			char msgunlock[41] = {
				0xfe, 0x21, 0xae, 0xff, 0xbe, 0x4c, 0, 0, 0x80, 0x3f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x90, 0x01, 0x01, 0x01, 0, 0xcd, 0xbb};
			char msglock[41] = {
				0xfe, 0x21, 0xb6, 0xff, 0xbe, 0x4c, 0, 0, 0x00, 0x00, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x90, 0x01, 0x01, 0x01, 0, 0x94, 0x5d};
			if (arm)
			{
#ifndef UDPMODE
				serial.async_write_some(buffer(msgunlock, 41), std::bind([]() {}));
#endif
			}
			else
			{
#ifndef UDPMODE
				serial.async_write_some(buffer(msglock, 41), std::bind([]() {}));
#endif
			}
		};
		/*
		arm_f(true);
------------------------------------------------------------------------------------------------------
*/
		arm_f(true);
		vector_state v_state;
		controler.SetReceiveFunc([&](vector<char> &data, UINT8 mark, ip::udp::endpoint ep) {
			//cout << "Mark:" << (int)mark << "data" << to_string(*((int*)v.data())) << endl;
			int n_mode_num;
			switch (mark)
			{

			case (0):
				if (string(data.data()) == "connect")
				{
					controler.remote_endpoint = ep;

					controler.send("connect", 0);
					printmsg("Connect from:" << ep.address().to_string() << ":" << ep.port());
					is_connected = true;
					controler.send(robotmessage, 1);
					close_connect_while_no_beat_for(1000);
					start_beating_until_close(300);
				}
				else if (string(data.data()) == "beat")
				{
					//printmsg("beat"<<ep.port());
					close_connect_while_no_beat_for(1000);
				}
				break;
			case (10):

				if (string(data.data()) == "load")
				{
					canloaddetector = !canloaddetector;
				}
				break;
			case (20):
				//printmsg("move msg");
				v_state = changePtrTotype(data.data(), vector_state);
				//controler.send("v:" + to_string(v_state.x) + " : " + to_string(v_state.y) + " : " + to_string(v_state.z) + " : " + to_string(v_state.r), 1);
				mavlink_msg_manual_control_send(chan, 1, v_state.x, v_state.y, v_state.z, v_state.r, 0);
				break;
			case (21):
				arm_f(true);
				break;
			case (22):
				arm_f(false);
				break;
			case (23):
				n_mode_num = changePtrTotype(data.data(), int);
				mavlink_msg_set_mode_send(chan, 1, 1, n_mode_num);
				break;
			case (100):
				changecamera(changePtrTotype(data.data(), int));
				controler.send("camera change to:" + to_string(changePtrTotype(data.data(), int)), 1);
				break;
			case (101):
				videofps = changePtrTotype(data.data(), int);
				controler.send("camera fps set to:" + to_string(videofps), 1);
				break;
			default:
				//printmsg("Unkonw mark:"<<(int)mark <<":"<<changePtrTotype(data.data(),int));
				controler.send("just send:" + string(data.data()) + " mark:" + to_string(mark), 1);
				break;
			}
		});

		/*

------------------------------------------------------------------------------------------------------
*/

		/*

------------------------------------------------------------------------------------------------------
*/
		cv::Mat u, q;
		atomic<bool> canusing(true);
		atomic<bool> newpic(false);
		std::thread netserver([&io]() {
			io.run();
		});

		printmsg("Start program");

		mavlink_msg_request_data_stream_send(chan, 1, 190, 0, 10, 1);
		asio::steady_timer msg_io(io);
		function<void(std::error_code)> check_stream = [&](std::error_code err) {
			if (battery_v == 0 && yawmsg == 0)
			{
				printmsg("no stream msg and try to request it again");
				mavlink_msg_request_data_stream_send(chan, 1, 190, 0, 10, 1);
				msg_io.expires_from_now(asio::chrono::seconds(1));
				msg_io.async_wait(check_stream);
			}
		};

		msg_io.expires_from_now(asio::chrono::seconds(1));
		msg_io.async_wait(check_stream);

		/*

------------------------------------------------------------------------------------------------------
*/
		long lasttime2 = clock();

		atomic<int> dtime2(0);
		atomic<int> dtime3(0);
		atomic<int> dtime4(0);
		bool can_detect = false;
		auto methfunc = [&]() {
			static cv::Mat detect_mat;
			using namespace std;
			using namespace cv;

#define w_width 830
#define h_hidth 650
			//	Mat tempshowMat;
			int step_state = 0;

			vector_state v_state;

			asio::steady_timer movekeeptimer(io);
			function<void(std::error_code)> movekeepfunc = [&](std::error_code err) {
				if (!err)
				{
					if (is_connected)
					{
						controler.send("r- X:" + to_string(v_state.x) + "Y:" + to_string(v_state.y) + "Z:" + to_string(v_state.z) + "R:" + to_string(v_state.r), 12);
					}
					if (!can_detect)
					{
						v_state.x = 0;
						v_state.y = 0;
						v_state.z = 0;
						v_state.r = 0;
					}
					mavlink_msg_manual_control_send(chan, 1, v_state.x, v_state.y, v_state.z, v_state.r, 0);

					movekeeptimer.expires_from_now(asio::chrono::milliseconds(500));
					movekeeptimer.async_wait(movekeepfunc);
				}
			};
			//x,y,z,r
			auto move = [&](int x, int y, int z, int r) {
				if (v_state.x != x || v_state.y != y || v_state.z != z || v_state.r != r)
				{
					v_state.x = x;
					v_state.y = y;
					v_state.z = z ;
					v_state.r = r;
					movekeepfunc(std::error_code());
				}
			};

			auto change_mode = [&chan](int modenum) {
				mavlink_msg_set_mode_send(chan, 1, 1, modenum);
			};
			auto set_depth_mode = [&change_mode, &deepmsg]() {
				if (deepmsg < -0.7)
				{
					return false;
				}
				else
				{
					change_mode(2);
					return true;
				}
			};
			auto box_in_rect = [](bbox_t &box, float wp, float hp) {
				Point a;
				Point b;
				a.x = 415 * wp;
				b.x = 830 - a.x;
				a.y = 325 * hp;
				b.y = 650 - a.y;
				///	printmsg("a,b,c,d:" << a.x << "  " << b.x << "  " << a.y << "  " << b.y);
				if (box.x >= a.x && box.y >= a.y && (box.x + box.w) <= b.x && (box.y + box.h) <= b.y)
					return true;
				return false;
			};
			auto box_warp_rect = [](bbox_t &box, float wp, float hp) {
				Point a;
				Point b;
				a.x = 415 * wp;
				b.x = 830 - a.x;
				a.y = 325 * hp;
				b.y = 650 - a.y;
				///	printmsg("a,b,c,d:" << a.x << "  " << b.x << "  " << a.y << "  " << b.y);
				if (box.x < a.x && box.y < a.y && (box.x + box.w) > b.x && (box.y + box.h) > b.y)
					return true;
				return false;
			};
			auto draw_by_p = [](Mat &mat_img, float wp, float hp, Scalar color) {
				Point a;
				Point b;
				a.x = 415 * wp;
				b.x = 830 - a.x;
				a.y = 325 * hp;
				b.y = 650 - a.y;
				cv::rectangle(mat_img, a, b, color, 2);
			};
			auto box_size = [](bbox_t &box) {
				if (box.w > box.h)
				{
					return box.w;
				}
				else
				{
					return box.h;
				}
			};
			auto box_center_destance = [](bbox_t &box) {
				Point p_center;
				p_center.x = box.x + box.w / 2;
				p_center.y = box.y + box.h / 2;
				p_center.x -= 415;
				p_center.y -= 325;
				return p_center;
			};
			auto find_bigest_box = [&box_size](std::vector<bbox_t> &boxes) {
				bbox_t *bigest_box = NULL;
				for (auto &i : boxes)
				{
					if (i.obj_id < 3)
					{
						if (!bigest_box || box_size(i) > box_size(*bigest_box))
							bigest_box = &i;
					}
				}
				return bigest_box;
			};
			enum box_prob
			{
				equal,
				too_big,
				not_in_rect,
				not_in_center,
				too_small,
				width_too_short,
				unkonw_probrom,
				no_msg
			};
			auto look_for = [&](bbox_t &box, float w1, float w2, float h1, float h2) {
				bool flag1 = box_in_rect(box, w1, h1);
				bool flag2 = box_warp_rect(box, w2, h2);
				float th1 = 1 - h1, th2 = 1 - h2, tw1 = 1 - w1, tw2 = 1 - w2;
				if (flag1 && flag2)
				{
					return equal;
				}
				if (box.w > w_width * tw1 || box.h > th1 * h_hidth)
				{
					return too_big;
				}
				if (!flag1)
				{
					return not_in_rect;
				}
				Point tmp_point = box_center_destance(box);
				if (tmp_point.x > 30 || tmp_point.x < -30 || tmp_point.y > 20 || tmp_point.y < -20)
				{
					return not_in_center;
				}
				if (box.h < h_hidth * th2)
				{
					return too_small;
				}
				if (box.w < w_width * tw2 || box.w < box.h * 0.8)
				{
					return width_too_short;
				}
				return unkonw_probrom;
			};

			asio::steady_timer task_timer_1(io);
			asio::steady_timer task_timer_2(io);
			auto taske_1_do = [&task_timer_1](int time, function<void(std::error_code)> func) {
				task_timer_1.expires_from_now(asio::chrono::milliseconds(time));
				task_timer_1.async_wait(bind(func, std::placeholders::_1));
			};
			auto taske_2_do = [&task_timer_2, &task_timer_1](int time, function<void(std::error_code)> func) {
				task_timer_2.expires_from_now(asio::chrono::milliseconds(time));
				task_timer_2.async_wait(bind(func, std::placeholders::_1));
			};

			//-------------------------------------------------------------------------------------------------------------------------------
#define safe_deep -0.9
			auto logic_func = [&](std::vector<bbox_t> &boxes) {
				if (!canloaddetector)
					return;
				//	static bool canstart = false;

				// if (deepmsg > safe_deep && deepmsg < -0.2)
				// {
				// 	if (custom_mode != 2)
				// 		set_depth_mode();
				// }
				// else if (deepmsg < safe_deep)
				// {
				// 	if (custom_mode != 0)
				// 		change_mode(0);
				// }
				// else if (deepmsg > -0.2)
				// {
				// 	if (custom_mode != 19)
				// 		change_mode(19);
				// 	move(0, 0, 0, 0);
				// }

				if (custom_mode != 2)
						change_mode(2);

				if (deepmsg < -0.3 && !can_detect)
				{
					can_detect = true;
				}
				if (deepmsg > -0.2 && can_detect)
				{
					can_detect = false;
				}
				if (!can_detect)
				{
					move(0, 0, 0, 0);
					return;
				}
				static function<void()> next_do = NULL;

				auto bigest_box = find_bigest_box(boxes);

				int boxnum = 0;
				for (auto &i : boxes)
				{
					if (i.obj_id < 3)
					{
						boxnum++;
					}
				}
				box_prob prob = no_msg;
				//-----------------------------------
				if (bigest_box != NULL)
				{
					prob = look_for(*bigest_box, 0.2, 0.55, 0.15, 0.4);
				}
				/*
					equal,
					too_big,
					not_in_rect,
					not_in_center,
					too_small,
					width_too_short,
					unkonw_probrom,
					no_msg
				*/

				static atomic<bool> zoom_to_box(false);
				static int theboxesnum = 0;
				bbox_t endball = {0};
				for (auto &boxq : boxes)
				{
					if (boxq.obj_id == 9)
					{
						endball = boxq;
					}
				}
				if (theboxesnum >= 4 && endball.obj_id != 9)
				{
					move(0, 0, 0, 190);
				}
				if (theboxesnum >= 4 && endball.obj_id == 9)
				{
					Point p_dp = box_center_destance(endball);
					int r = 0, z = 0;
					if (p_dp.x < -50)
					{
						r = -190;
					}
					else if (p_dp.x > 50)
					{
						r = 190;
					}
					if (p_dp.y < -30)
					{
						z = 170;
					}
					else if (p_dp.y > 30)
					{
						z = -100;
					}

					if (z == 0 && r == 0)
					{
						move(210, 0, 0, 0);
					}
					else
					{
						move(0, 0, z, r);
					}
				}

				if (!zoom_to_box && prob != no_msg && bigest_box != NULL && theboxesnum < 4)
				{
					if (prob == equal)
					{
						taske_1_do(1500, [&](std::error_code err) {
							//printmsg("cd cingshiq 1");
							if (!err)
							{
								zoom_to_box = true;
								move(250, 0, 0, 0);
								taske_2_do(4000, [&](std::error_code err) {
									//printmsg("cd cingshiq 2");
									if (!err)
									{
										theboxesnum++;
										zoom_to_box = false;
										move(0, 0, 0, 0);
									}
								});
							}
						});
					}
					if (prob == too_big)
					{
						move(-150, 0, 0, 0);
					}
					if (prob == not_in_rect)
					{
						Point p_dp = box_center_destance(*bigest_box);
						int r = 0, z = 0;
						if (p_dp.x < -50)
						{
							r = -200;
						}
						else if (p_dp.x > 50)
						{
							r = 200;
						}
						if (p_dp.y < -30)
						{
							z = 170;
						}
						else if (p_dp.y > 30)
						{
							z = -100;
						}
						move(0, 0, z, r);
					}
					if (prob == not_in_center)
					{
						Point p_dp = box_center_destance(*bigest_box);
						int r = 0, z = 0;
						if (p_dp.x < -50)
						{
							r = -190;
						}
						else if (p_dp.x > 50)
						{
							r = 190;
						}
						if (p_dp.y < -30)
						{
							z = 170;
						}
						else if (p_dp.y > 30)
						{
							z = -100;
						}
						move(0, 0, z, r);
					}
					if (prob == too_small)
					{
						move(180, 0, 0, 0);
					}
					if (prob == width_too_short)
					{
						if (bigest_box->obj_id == 1)
						{
							//++++++++++++++
							/*
							taskmode = true;
							printmsg("��ʼ����");
							taske_1_do(2000, [&](std::error_code err) {
								printmsg("q_a_q");
								if (!err) {
									printmsg("���ڽ���");
									move(0, 0, 0, 0);
									taskmode = false;
								}
								}
							);*/
							move(0, 220, 0, 0);
						}
						else if (bigest_box->obj_id == 2)
						{
							//---------------
							/*
							taskmode = true;
							printmsg("��ʼ����");
							taske_1_do(2000, [&](std::error_code err) {
								printmsg("q_b_q");
								if (!err) {
									move(0, 0, 0, 0);
									printmsg("���ڽ���");
									taskmode = false;
								}
								}
							); */

							move(0, -220, 0, 0);
						}
					}
				}
			};

			while (1)
			{
				
				if (detector && canusing && canloaddetector)
				{
					lasttime2 = clock();
					// printmsg("detect: "<< newpic);
					if (newpic)
					{
						canusing = false;
						detect_mat = q.clone();
						canusing = true;
						newpic = false;
						dtime2 = clock() - lasttime2;
						//printmsg(detect_mat.empty());
						//printmsg(detect_mat.empty());
						if (!detect_mat.empty())
						{
							auto boxes = detector->detect(detect_mat, 0.4);
							if (is_connected)
							{
								controler.send((char *)boxes.data(), sizeof(bbox_t) * boxes.size(), 33);
								//	printmsg(boxes.size());
							}
							dtime3 = clock() - lasttime2;
							// logic_func(boxes);
							move(0, 100, -500, 0);
							//move(0, -100, -500, 0   );

							dtime4 = clock() - lasttime2;
						}
					}
				}
				else
				{

					Sleep(2);
				}
			}
		};

		std::thread detector_thread(methfunc);
		/*

		------------------------------------------------------------------------------------------------------
		*/

		long lasttime1 = clock();
		int dtime1 = 0;
		arm_f(1);
		while (1)
		{

			if (camera->isOpened())
			{
				camera->operator>>(u);
				dtime1 = clock() - lasttime1;
				lasttime1 = clock();
				remap(u, u, map1, map2, INTER_LINEAR);

				if (canusing)
				{
					canusing = false;
					q = u(Range(50, 700), Range(85, 915));
					canusing = true;
					newpic = true;
				}
				else
				{
					Sleep(1);
					continue;
				}

				//-------------------------------
				if (is_connected)
				{
					if (!mav_errmsg.empty() && mav_errmsg != "")
					{
						controler.send(mav_errmsg, 4);
						mav_errmsg = "";
					}
					zitaimsg.pitch = pitchmsg;
					zitaimsg.roll = rollmsg;
					zitaimsg.yaw = yawmsg;
					zitaimsg.deep = deepmsg;
					controler.send((char *)&zitaimsg, sizeof(zitaimsg), 5);
					int tn = custom_mode;
					controler.send((char *)&tn, sizeof(tn), 6);
					tn = base_mode;
					controler.send((char *)&tn, sizeof(tn), 11);
					//controler.send("mode(down): "+to_string(n_mode), 1);
					tn = battery_v;
					controler.send((char *)&tn, sizeof(tn), 7);
					tn = dtime1 * dxnum;
					controler.send((char *)&tn, sizeof(tn), 8);
					tn = dtime2 * dxnum;
					controler.send((char *)&tn, sizeof(tn), 9);
					tn = dtime3 * dxnum;
					controler.send((char *)&tn, sizeof(tn), 31);
					tn = dtime4 * dxnum;
					controler.send((char *)&tn, sizeof(tn), 32);
					static vector<uchar> k;
					canusing = false;
					cv::imencode(".jpg", q, k);
					canusing = true;
					controler.send((char *)k.data(), k.size(), 3);
				}
			}
			//Sleep(2);
		}
		detector_thread.join();
		netserver.join();
	}
	catch (std::exception &e)
	{
		std::cerr << e.what() << std::endl;
	}

	return 0;
}
